#include <iostream>
#include <fstream>
#include <string>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <vector>



#include "cv.h"
#include "highgui.h"

//#include <opencv.hpp>



//#include "ros/ros.h"

#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/visualization/cloud_viewer.h>



//#include <sensor_msgs/PointCloud2.h>
//#include <std_msgs/Int32.h>

#include <Eigen/Core> 
#include <Eigen/Geometry>



#include "graph/VertexCamera.cpp"
#include "graph/VertexPlane.cpp"
#include "graph/VertexPoint3D.cpp"
#include "graph/EdgeCameraPlane.cpp"



#include "FeatureDescriptor.cpp"
#include "SurfFeatureDescriptor64.cpp"
#include "Histogram.cpp"
#include "RGBFeatureDescriptor.cpp"



#include "Point.cpp"
#include "Plane.cpp"
#include "Line.cpp"


//#include "cvap_6D_slam/kinect_data.h" // kinect_data msg

#include <string.h>
#include "KeyPoint.cpp"
#include "KeyPointSet.cpp"

#include "FeatureExtractor.cpp"
#include "SurfExtractor.cpp"
#include "EdgeDetector.cpp"
#include "segmentation.cpp"
#include "RGBDFrame.cpp"

#include "Association.cpp"
#include "KeyPointAssociation.cpp"
#include "PlaneAssociation.cpp"
#include "AssociationSet.cpp"

#include "FrameMatcher.cpp"
#include "DummyMatcher.cpp"
#include "SurfRansacMatcherI.cpp"
#include "JCMergeMatcherI.cpp"
#include "JCSearchMatcherI.cpp"
#include "BasicIcpMatcher.cpp"
#include "BasicGIcpMatcher.cpp"
#include "KeyPointIcpMatcherI.cpp"
#include "Map.cpp"

#include <pthread.h>
#include <sys/types.h>
#include <dirent.h>
#include <errno.h>




cvap_6D_slam::Map * mymap;


using namespace std;

string output_path;

struct Test_params {
	string path;
	string path_gt;
	string path_asso;
	string prefix;

	int max_backing;
	int calibration;
	int max_nr_frames;
	vector<FrameMatcher * > matchers;
};

struct Freib_data {
	double timestamp;
	double tx;
	double ty;
	double tz;
	double qx;
	double qy;
	double qz;
	double qw;
};

struct Timestamp_path {
	double timestamp;
	string path;
};

struct Frame_data {
	Timestamp_path * rgb;
	Timestamp_path * depth;
	RGBDFrame * frame;
};

struct Analyzation_data {
	float val_time;
	Frame_data * src;
	Frame_data * dst;
	Transformation * transformation;
};

struct Test_task {
	FrameMatcher * matcher;
	int matcher_id;
	string filename;
	Analyzation_data * result;
};

pthread_mutex_t tasks_mutex = PTHREAD_MUTEX_INITIALIZER;
vector<Test_task * > * tasks;

pthread_mutex_t done_tasks_mutex = PTHREAD_MUTEX_INITIALIZER;
int done_tasks = 0;

pcl::PointCloud<pcl::PointXYZRGB>::Ptr generatePC(Frame_data * fd, int calibration)
{

	IplImage* rgb_img 	= cvLoadImage(fd->rgb->path.c_str(),CV_LOAD_IMAGE_UNCHANGED);
	char * rgb_data		= rgb_img->imageData;
	
	IplImage* depth_img = cvLoadImage(fd->depth->path.c_str(),CV_LOAD_IMAGE_UNCHANGED);
	short * depth_data	= (short *)depth_img->imageData;
	
	
	
	IplImage * test_img 	= cvCreateImage(cvSize(rgb_img->width, rgb_img->height), IPL_DEPTH_32F, 3);
	float focalLengthX;
	float focalLengthY;
	float centerX;
	float centerY;
	float scalingFactor = 5000.0;
	float depthScaling;

	if(calibration == 0){
		focalLengthX = 525.0;
		focalLengthY = 525.0;
		centerX = 319.5;
		centerY = 239.5;
		depthScaling = 1;
	}else if(calibration == 1){
	 	 	 	
		focalLengthX = 517.3f;
		focalLengthY = 516.5f;
		centerX = 318.6f;
		centerY = 255.3f;
		depthScaling = 1.035f;
	}

	float invScaling = 1/scalingFactor;
	float invFocalX = 1/focalLengthX;
	float invFocalY = 1/focalLengthY;
	
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
	cloud->width    = rgb_img->width;
	cloud->height   = rgb_img->height;
	//cloud.is_dense = false;
	cloud->points.resize (cloud->width * cloud->height);
	int point_index = 0;
	
	for(int j = 0; j < rgb_img->height; j++){
		for(int i = 0; i < rgb_img->width; i++){
			int ind_rgb   = j * (8*rgb_img->widthStep/rgb_img->depth/rgb_img->nChannels) + i;
			//int ind_depth = j * (8*rgb_img->widthStep/rgb_img->depth/depth_img->nChannels) + i;
			int ind_test  = j * (8*test_img->widthStep/test_img->depth/test_img->nChannels) + i;
			//float current_depth = float(depth_data[2*ind]);
			
			int r = char(rgb_data[3*ind_rgb+2]);
			int g = char(rgb_data[3*ind_rgb+1]);
			int b = char(rgb_data[3*ind_rgb+0]);
			int d = short(depth_data[1*ind_test+0]);
			if(r < 0){r = 255+r;}
			if(g < 0){g = 255+g;}
			if(b < 0){b = 255+b;}
			float fr = float(r);
			float fg = float(g);
			float fb = float(b);
			float fd = float(d);
			
			float x = 0;
			float y = 0;
			float z = fd * invScaling*depthScaling;

			if(z > 0){
				x = (i - centerX) * z * invFocalX;
            	y = (j - centerY) * z * invFocalY;
			}
			cloud->points[point_index].x = x;
			cloud->points[point_index].y = y;
			cloud->points[point_index].z = z;
			cloud->points[point_index].r = fr;
			cloud->points[point_index].g = fg;
			cloud->points[point_index].b = fb;
			
			point_index++;
		}
	}
	cvReleaseImage( &rgb_img );
	cvReleaseImage( &depth_img );
	cvReleaseImage( &test_img );
	return cloud;
}

void print_fd(Frame_data * data)
{
	printf("-------------------------------------------\n");
	printf("D timestamp: %f\n",data->depth->timestamp);
	printf("D: %s\n" ,data->depth->path.c_str());
	printf("C timestamp: %f\n",data->rgb->timestamp);
	printf("C: %s\n" ,data->rgb->path.c_str());

}

void print_gt(Freib_data * gt)
{
	printf("-------------------------------------------\n");
	printf("timestamp: %f\n",gt->timestamp);
	printf("tx: %f ",gt->tx);
	printf("ty: %f ",gt->ty);
	printf("tz: %f\n",gt->tz);
	printf("qx: %f ",gt->qx);
	printf("qy: %f ",gt->qy);
	printf("qz: %f ",gt->qz);
	printf("qw: %f\n",gt->qw);
}

/*
void print_tp(timestamp_path * tp)
{
	printf("-------------------------------------------\n");
	printf("timestamp: %f\n",tp->timestamp);
	printf("path: %s\n",tp->path.c_str());
}
*/

void test_a_task(Test_task * t)
{
		Frame_data * src_fd = t->result->src;
		Frame_data * dst_fd = t->result->dst;
		
		struct timeval start, end;
		gettimeofday(&start, NULL);
		Transformation * trans = t->matcher->getTransformation(src_fd->frame, dst_fd->frame);
		gettimeofday(&end, NULL);
/*
		g2o::SE3Quat src_poseSE3 (Eigen::Quaterniond(src_fd->gt->qw, src_fd->gt->qx, src_fd->gt->qy, src_fd->gt->qz), Eigen::Vector3d(src_fd->gt->tx, src_fd->gt->ty, src_fd->gt->tz));
		g2o::SE3Quat dst_poseSE3 (Eigen::Quaterniond(dst_fd->gt->qw, dst_fd->gt->qx, dst_fd->gt->qy, dst_fd->gt->qz), Eigen::Vector3d(dst_fd->gt->tx, dst_fd->gt->ty, dst_fd->gt->tz));
				
		Eigen::Matrix4f src_mat = src_poseSE3.to_homogenious_matrix().cast<float>();
		Eigen::Matrix4f dst_mat = dst_poseSE3.to_homogenious_matrix().cast<float>();
				
		Eigen::Matrix4f true_trans_mat = dst_mat.inverse()*src_mat;
		if(trans == NULL){
			trans = new Transformation();
			trans->src = src_fd->frame;
			trans->dst = dst_fd->frame;
			trans->transformationMatrix = true_trans_mat;
			trans->weight = 100000;
		}
*/
		t->result->val_time			= (end.tv_sec*1000000+end.tv_usec-(start.tv_sec*1000000+start.tv_usec))/1000000.0f;
		t->result->transformation	= trans;
		
}


int nr_done_tasks(){
    pthread_mutex_lock(&done_tasks_mutex);
	int val = done_tasks;
	pthread_mutex_unlock(&done_tasks_mutex);
	return val;
}

void * start_test_thread( void *ptr )
{
	while(true)
	{
		pthread_mutex_lock( &tasks_mutex );
		if(tasks->size() > 0)
		{
			Test_task * t = tasks->back();
			tasks->pop_back();
			pthread_mutex_unlock(&tasks_mutex);			
			test_a_task(t);

			string path = output_path+t->filename;
			ifstream task_file (path.c_str());
			if (task_file.is_open()){
				task_file.seekg(0,ifstream::end);
				long size=task_file.tellg();
				char * buffer_char 		= new char [size];
				int * buffer_int 		= (int *) buffer_char;
				float * buffer_float	= (float *) buffer_char;
				double * buffer_double	= (double *) buffer_char;

				task_file.seekg(0);
				task_file.read (buffer_char,size);
				task_file.close();

				buffer_int[3] = 1;

				
				buffer_float[4] = t->result->transformation->transformationMatrix(0,0);
				buffer_float[5] = t->result->transformation->transformationMatrix(0,1);
				buffer_float[6] = t->result->transformation->transformationMatrix(0,2);
				buffer_float[7] = t->result->transformation->transformationMatrix(0,3);
				
				buffer_float[8] = t->result->transformation->transformationMatrix(1,0);
				buffer_float[9] = t->result->transformation->transformationMatrix(1,1);
				buffer_float[10] = t->result->transformation->transformationMatrix(1,2);
				buffer_float[11] = t->result->transformation->transformationMatrix(1,3);
				
				buffer_float[12] = t->result->transformation->transformationMatrix(2,0);
				buffer_float[13] = t->result->transformation->transformationMatrix(2,1);
				buffer_float[14] = t->result->transformation->transformationMatrix(2,2);
				buffer_float[15] = t->result->transformation->transformationMatrix(2,3);
				
				buffer_float[16] = t->result->transformation->transformationMatrix(3,0);
				buffer_float[17] = t->result->transformation->transformationMatrix(3,1);
				buffer_float[18] = t->result->transformation->transformationMatrix(3,2);
				buffer_float[19] = t->result->transformation->transformationMatrix(3,3);
				
				//for(int i = 4; i < 20; i++){printf("%f ",buffer_float[i]);}printf("\n");
				
				buffer_double[10] = t->result->src->depth->timestamp;
				buffer_double[11] = t->result->dst->depth->timestamp;
				buffer_double[12] = t->result->val_time;
				buffer_double[13] = t->result->transformation->weight;
								
				ofstream outfile (path.c_str(),ofstream::binary);
				outfile.write (buffer_char,size);
				outfile.close();
				delete buffer_char;
				
				pthread_mutex_lock(&done_tasks_mutex);
				done_tasks++;
				pthread_mutex_unlock(&done_tasks_mutex);
			}
		}else{
			pthread_mutex_unlock(&tasks_mutex);
			usleep(1000);
		}
	}
}

int getdir (string dir, vector<string> &files)
{
    DIR *dp;
    struct dirent *dirp;
    if((dp  = opendir(dir.c_str())) == NULL) {
        cout << "Error(" << errno << ") opening " << dir << endl;
        return errno;
    }

    while ((dirp = readdir(dp)) != NULL) {
        files.push_back(string(dirp->d_name));
    }
    closedir(dp);
    return 0;
}

bool mycomparison (Test_task * i,Test_task * j) {return (i->result->src->frame->id - i->result->dst->frame->id) > (j->result->src->frame->id - j->result->dst->frame->id);}

void generate(Test_params * tp)
{
	printf("starting setup\n");
	
	string line;
	ifstream gt_file (tp->path_gt.c_str());
	vector< Freib_data * > gt_vec;
	if (gt_file.is_open()){
		while ( gt_file.good() )
		{
			getline (gt_file,line);
			if(line[0] != '#'){
				int space1 = line.find(" ");
				if(space1 != -1){
					
					int space2 = line.find(" ",space1+1,1);
					int space3 = line.find(" ",space2+1,1);
					int space4 = line.find(" ",space3+1,1);
					int space5 = line.find(" ",space4+1,1);
					int space6 = line.find(" ",space5+1,1);
					int space7 = line.find(" ",space6+1,1);
					
					Freib_data * gt = new Freib_data();
					gt->timestamp 	= atof(line.substr(0,space1).c_str());
					gt->tx 			= atof(line.substr(space1+1,space2-space1-1).c_str());
					gt->ty 			= atof(line.substr(space2+1,space3-space2-1).c_str());
					gt->tz 			= atof(line.substr(space3+1,space4-space3-1).c_str());
					gt->qx 			= atof(line.substr(space4+1,space5-space4-1).c_str());
					gt->qy 			= atof(line.substr(space5+1,space6-space5-1).c_str());
					gt->qz 			= atof(line.substr(space6+1,space7-space6-1).c_str());
					gt->qw 			= atof(line.substr(space7+1,line.length()-space7-1).c_str());
					gt_vec.push_back(gt);
				}
			}
		}
		gt_file.close();
	}else{cout << "Unable to open " << tp->path_gt;}
	
	vector<FeatureDescriptor *> * centers = new vector<FeatureDescriptor *>();
	ifstream asso_file (tp->path_asso.c_str());
	vector< Frame_data * > frame_vec;
	

	if (asso_file.is_open()){
		while ( asso_file.good() )
		{
			getline (asso_file,line);
			if(line[0] != '#'){
				
				int space1 = line.find(" ");
				if(space1 != -1 && frame_vec.size() < tp->max_nr_frames){
					int space2 = line.find(" ",space1+1,1);
					int space3 = line.find(" ",space2+1,1);
					int space4 = line.find(" ",space3+1,1);
					printf("%s\n",line.c_str());
					Frame_data * frame 		= new Frame_data();
					frame->rgb 				= new Timestamp_path();
					frame->depth			= new Timestamp_path();
					frame->depth->timestamp	= atof(line.substr(0,space1).c_str());
					frame->depth->path 		= tp->path+"/"+line.substr(space1+1,space2-space1-1);
					frame->rgb->timestamp	= atof(line.substr(space2+1,space3-space2-1).c_str());
					frame->rgb->path 		= tp->path+"/"+line.substr(space3+1,space4-space3-1);
					print_fd(frame);
					frame_vec.push_back(frame);
				}
			}
		}
		asso_file.close();
	}else{cout << "Unable to open " << tp->path_asso;}
	
	for(int i = 0; i < frame_vec.size(); i++)
	{
		printf("%i\n",int(i));
		Frame_data * frame = frame_vec.at(i);
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = generatePC(frame,tp->calibration);
		pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
		pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
		ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
		ne.setMaxDepthChangeFactor(0.02f);
		ne.setNormalSmoothingSize(5.0f);
		ne.setInputCloud(cloud);
		ne.compute(*normals);
		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr full_cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
		pcl::concatenateFields(*cloud,*normals,*full_cloud);
		cloud.reset();
		normals.reset();
		frame->frame = new RGBDFrame(full_cloud,centers);
	}	
	
	for(int i = 0; i < (int)frame_vec.size(); i++)
	{
		for(int j = i-1; j >=  max(0,i -tp->max_backing); j--){
			for(unsigned int k = 0; k < tp->matchers.size(); k++)
			{
				char fpath [150];
				sprintf(fpath,"%s%s_%i_%i_%i.task",output_path.c_str(),tp->prefix.c_str(),i,j,k);
				ifstream task_file (fpath);
				if (!task_file.is_open()){
					long size 				= sizeof(int)*(4)+sizeof(float)*(16)+sizeof(double)*(4);
					char * buffer_char 		= new char[size];
					int * buffer_int 		= (int *)buffer_char;
					float * buffer_float	= (float *)buffer_char;
					double * buffer_double	= (double *)buffer_char;
					
					buffer_int[0] = i;
					buffer_int[1] = j;
					buffer_int[2] = k;
					buffer_int[3] = 0;
					
					buffer_float[4 ] = 0;
					buffer_float[5 ] = 0;
					buffer_float[6 ] = 0;
					buffer_float[7 ] = 0;
					
					buffer_float[8 ] = 0;
					buffer_float[9 ] = 0;
					buffer_float[10] = 0;
					buffer_float[11] = 0;
					
					buffer_float[12] = 0;
					buffer_float[13] = 0;
					buffer_float[14] = 0;
					buffer_float[15] = 0;
					
					buffer_float[16] = 0;
					buffer_float[17] = 0;
					buffer_float[18] = 0;
					buffer_float[19] = 0;
					
					buffer_double[10] = 0;
					buffer_double[11] = 0;
					buffer_double[12] = 0;
					buffer_double[13] = 0;
										
					ofstream outfile (fpath,ofstream::binary);
					outfile.write (buffer_char,size);
					outfile.close();
					delete buffer_char;
				}else{task_file.close();}
			}
		}
	}

	tasks = new vector<Test_task * >();
	
	vector<string> files = vector<string>();
	getdir(output_path,files);
	int added_tasks = 0;
    for (unsigned int i = 0;i < files.size();i++) {
        if(files.at(i).find(tp->prefix)==0 && files.at(i).find("_") == tp->prefix.length()){
        	ifstream task_file ((output_path+files.at(i)).c_str());
			if (task_file.is_open()){
				
				task_file.seekg(0,ifstream::end);
				long size=task_file.tellg();
				char * buffer_char 		= new char [size];
				int * buffer_int 		= (int *) buffer_char;
				float * buffer_float	= (float *) buffer_float;
				//float * buffer_double	= (float *) buffer_double;
				task_file.seekg(0);
				task_file.read (buffer_char,size);
				task_file.close();

				printf("1->%f---->%i %i %i %i\n",float(i)/float(files.size()),buffer_int[0],buffer_int[1],buffer_int[2],buffer_int[3]);
				if(buffer_int[3] == 0){
					printf("creating task\n");
					Frame_data * src_fd = frame_vec.at(buffer_int[0]);
					Frame_data * dst_fd = frame_vec.at(buffer_int[1]);
					Test_task * t 		= new Test_task();
					t->result 			= new Analyzation_data();
					t->result->src 		= src_fd;
					t->result->dst 		= dst_fd;
					t->result->transformation = 0;
					t->result->val_time = 0;
					t->matcher 			= tp->matchers.at(buffer_int[2]);
					t->matcher_id 		= buffer_int[2];
					t->filename 		= string(files.at(i));
					
					pthread_mutex_lock( &tasks_mutex );
					tasks->push_back(t);
					pthread_mutex_unlock(&tasks_mutex);
					added_tasks++;
				}
				delete buffer_char;
        	}
        }
    }
    
    pthread_mutex_lock( &tasks_mutex );
	sort(tasks->begin(),tasks->end(),mycomparison);
	pthread_mutex_unlock(&tasks_mutex);
	
    for(int i = 0; i < 6; i++){
		pthread_t mythread;
		pthread_create( &mythread, NULL, start_test_thread, NULL);
	}
    
    struct timeval test_start, test_end;
	gettimeofday(&test_start, NULL);
	while(nr_done_tasks() < added_tasks){
		gettimeofday(&test_end, NULL);
		float test_time = (test_end.tv_sec*1000000+test_end.tv_usec-(test_start.tv_sec*1000000+test_start.tv_usec))/1000000.0f;
		printf("%i/%i Time spent: %f\n",nr_done_tasks(),added_tasks,test_time);
		usleep(500000);
	}
	gettimeofday(&test_end, NULL);
	float test_time = (test_end.tv_sec*1000000+test_end.tv_usec-(test_start.tv_sec*1000000+test_start.tv_usec))/1000000.0f;
	printf("%i/%i Time spent: %f\n",nr_done_tasks(),added_tasks,test_time);
}

void analyze(Test_params * tp){
	g2o::SparseOptimizer graphoptimizer;
	graphoptimizer.setMethod(g2o::SparseOptimizer::LevenbergMarquardt);
	graphoptimizer.setVerbose(false);
		
	g2o::BlockSolver_6_3::LinearSolverType * linearSolver = new g2o::LinearSolverPCG<g2o::BlockSolver_6_3::PoseMatrixType>();
	g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(&graphoptimizer,linearSolver);

	graphoptimizer.setSolver(solver_ptr);

	printf("done\n");
	
	int max_node = 0;
	vector<double> timestamps;
	vector<string> files = vector<string>();
	getdir(output_path,files);
	for (unsigned int i = 0;i < files.size();i++) {
		if(files.at(i).find(tp->prefix)==0 && files.at(i).find("_") == tp->prefix.length()){
			ifstream task_file ((output_path+files.at(i)).c_str());
			if (task_file.is_open()){
				task_file.seekg(0,ifstream::end);
				long size=task_file.tellg();
				char * buffer_char 		= new char [size];
				int * buffer_int 		= (int *) buffer_char;
				float * buffer_float	= (float *) buffer_char;
				double * buffer_double	= (double *) buffer_char;
				task_file.seekg(0);
				task_file.read (buffer_char,size);
				task_file.close();
				printf("-----------------------------------------------------\n");
				printf("2->size: %i\n",size);
				printf("2->%f---->%i %i %i %i\n",float(i)/float(files.size()),buffer_int[0],buffer_int[1],buffer_int[2],buffer_int[3]);
				int src_id = buffer_int[0];
				int dst_id = buffer_int[1];
				
				while(timestamps.size() <= src_id){timestamps.push_back(-1);printf("%i\n",timestamps.size());}
				while(timestamps.size() <= dst_id){timestamps.push_back(-1);printf("%i\n",timestamps.size());}
				
				if(timestamps.at(src_id) < 0){
					Eigen::Affine3d eigenTransform(Eigen::Matrix4d::Identity());
					g2o::SE3Quat poseSE3(Eigen::Quaterniond(eigenTransform.rotation()),Eigen::Vector3d(eigenTransform(0,3), eigenTransform(1,3), eigenTransform(2,3)));
					g2o::VertexSE3 * vertexSE3 = new g2o::VertexSE3();
					vertexSE3->setId(src_id);
					vertexSE3->estimate() = poseSE3;
					if(src_id == 0){vertexSE3->setFixed(true);}
					graphoptimizer.addVertex(vertexSE3);
				}
				if(timestamps.at(dst_id) < 0){
					Eigen::Affine3d eigenTransform(Eigen::Matrix4d::Identity());
					g2o::SE3Quat poseSE3(Eigen::Quaterniond(eigenTransform.rotation()),Eigen::Vector3d(eigenTransform(0,3), eigenTransform(1,3), eigenTransform(2,3)));
					g2o::VertexSE3 * vertexSE3 = new g2o::VertexSE3();
					vertexSE3->setId(dst_id);
					vertexSE3->estimate() = poseSE3;
					if(dst_id == 0){vertexSE3->setFixed(true);}
					graphoptimizer.addVertex(vertexSE3);
				}
				timestamps.at(src_id) = buffer_double[10];
				timestamps.at(dst_id) = buffer_double[11];
				
				Eigen::Matrix4d transformationMatrix = Eigen::Matrix4d::Identity();
				transformationMatrix(0,0) = buffer_float[4];
				transformationMatrix(0,1) = buffer_float[5];
				transformationMatrix(0,2) = buffer_float[6];
				transformationMatrix(0,3) = buffer_float[7];
				
				transformationMatrix(1,0) = buffer_float[8];
				transformationMatrix(1,1) = buffer_float[9];
				transformationMatrix(1,2) = buffer_float[10];
				transformationMatrix(1,3) = buffer_float[11];
				
				transformationMatrix(2,0) = buffer_float[12];
				transformationMatrix(2,1) = buffer_float[13];
				transformationMatrix(2,2) = buffer_float[14];
				transformationMatrix(2,3) = buffer_float[15];
				
				transformationMatrix(3,0) = buffer_float[16];
				transformationMatrix(3,1) = buffer_float[17];
				transformationMatrix(3,2) = buffer_float[18];
				transformationMatrix(3,3) = buffer_float[19];
				
				printf("src timestamp:%f \n",buffer_double[10]);
				printf("dst timestamp:%f \n",buffer_double[11]);
				printf("eval_time:%f \n",buffer_double[12]);
				printf("weight:%f \n",buffer_double[13]);
				
				delete buffer_char;
				Eigen::Affine3d eigenTransform(transformationMatrix);
				g2o::SE3Quat transfoSE3(Eigen::Quaterniond(eigenTransform.rotation()),Eigen::Vector3d(eigenTransform(0,3), eigenTransform(1,3), eigenTransform(2,3)));
				g2o::EdgeSE3* edgeSE3 = new g2o::EdgeSE3;	
				edgeSE3->vertices()[0] = graphoptimizer.vertex(src_id);
				edgeSE3->vertices()[1] = graphoptimizer.vertex(dst_id);
				edgeSE3->setMeasurement(transfoSE3.inverse());
				edgeSE3->setInverseMeasurement(transfoSE3);
				Eigen::Matrix<double, 6, 6, 0, 6, 6> mat;
				mat.setIdentity(6,6);
				edgeSE3->information() = mat;
				graphoptimizer.addEdge(edgeSE3);
        	}
        }
    }

	graphoptimizer.initializeOptimization();
	graphoptimizer.setVerbose(true);
	graphoptimizer.optimize(30);
	
	for(int i  = 0; i < timestamps.size(); i++){
		g2o::VertexSE3 * vertexSE3_src = (g2o::VertexSE3*)(graphoptimizer.vertex(i));
		Eigen::Matrix4f matrix = (vertexSE3_src->estimate().to_homogenious_matrix()).cast<float>();
		Eigen::Affine3f eigenTransform(matrix);
		Eigen::Quaternionf q = Eigen::Quaternionf(eigenTransform.rotation());
		printf("%f %f %f %f %f %f %f %f\n",timestamps.at(i),eigenTransform(0,3), eigenTransform(1,3), eigenTransform(2,3),q.x(),q.y(),q.z(),q.w());
	}
}

int main(int argc, char **argv)
{
	Test_params * tp = new Test_params();
	/*
	tp->path = "/home/johane/test_data/rgbd_dataset_freiburg1_room";
	tp->path_gt = tp->path+"/groundtruth.txt";
	tp->path_asso = tp->path+"/asso.txt";
	tp->prefix = "firstTest";
	
	tp->max_backing = 1;
	tp->calibration = 0;
	tp->max_nr_frames = 100;

	KeyPointIcpMatcherI * m = new KeyPointIcpMatcherI();
	m->nr_iter = 150;
	m->shrinking = 0.95f;
	m->stabilety_threshold = 0.1f*0.001f;
	tp->matchers.push_back(m);
	
	output_path = "freib_output/";
	generate(tp);
	*/
	tp->path = "/home/johane/test_data/rgbd_dataset_freiburg1_room";
	tp->path_gt = tp->path+"/groundtruth.txt";
	tp->path_asso = tp->path+"/asso.txt";
	tp->prefix = "firstTest";
	
	tp->max_backing = 1;
	tp->calibration = 0;
	tp->max_nr_frames = 100;

	KeyPointIcpMatcherI * m = new KeyPointIcpMatcherI();
	m->nr_iter = 150;
	m->shrinking = 0.95f;
	m->stabilety_threshold = 0.1f*0.001f;
	tp->matchers.push_back(m);
	
	output_path = "freib_output/";
	analyze(tp);
	return 0;
}
